/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
*  \class	TGroundModelMap
*  \author 	Jari Saarinen
*  \date 	Thu Jun 15 11:59:04 2006

*  \version: 0.1  
*/

#ifndef T_GROUND_MODEL_MAP_H
#define T_GROUND_MODEL_MAP_H


#include<stdio.h>
#include "CLine.h"
#include "TAngle.h"
#include "TMetricGrid.h"

// For main
//#define T_GROUND_MODEL_MAP_DEBUG

class TGroundModelMap : public TMetricGrid
{
private: 
	
public:
		///Constrains for the x,y,z values (the datapoint must be within this)
		float xMinConst;
		float xMaxConst;
		float yMinConst;
		float yMaxConst;
		float zMinConst;
		float zMaxConst;
		
		 
	TGroundModelMap();
	~TGroundModelMap();
	TGroundModelMap(float size_x, float size_y, float reso, TPixelCoord origin);
	void initGroundModelMap(float size_x, float size_y, float reso, TPixelCoord origin);
	
	
	
	/**
	* Set the whole map into some initial value 
	* @param value	The value that is copied to whole map
	*/
	void setGroundModelMapToInitialValue(int value);
	
	/**
	* get height value from the map
	* @param P	The metric position of the height
	* @return The height in that position in meters 
	*/
	float getHeight(TCoord2D P);


	/**
	* Returns the height from the grid position P (in pixels) 
	* @param P	the pixel coord 
	* @return the height from ther pixel coord P
	*/
	float getHeight(TPixelCoord P);
	
	/**
	 * Check if the index is out of bounds
	 **/
	bool isIndexOutOfBounds(int x, int y);
	
	/**
	* Set height @h value to the map into some metric position @P
	* @param P the position
	* @param h the height value
	* 
	*/ 
	int setHeight(TCoord2D P, float height);

	/**
	* Set height @h value to the map into grid position TPixelCoord @P
	* @param P the pixel coordinate
	* @param h the height value
	* 
	*/ 
	int setHeight(TPixelCoord P, float height);
	
	/**
	* Updates the value to the map. If there already is a value the new value is an average of 
	* old and new value.
	* @param P		The position of the updated height
	* @param height	The height to be updated
	* @return the updated value	
	*/
	float updateHeight(TCoord2D P, float height);
	
	/**
	* Updates the value to the map. If there already is a value the new value is the minimum from  
	* old and new values.
	* @param P		The position of the updated height
	* @param height	The height to be updated
	* @return the updated value	
	*/
	float updateMinimumHeight(TCoord2D P, float height);
	
/**
	 * Updates the value to the map. If there already is a value the new value is the Maximum from  
	 * old and new values.
	 * @param P		The position of the updated height
	 * @param height	The height to be updated
	 * @return the updated value	
 */
	float updateMaximumHeight(TCoord2D P, float height);
	
	/**
	* Returns an average value that surround the point @P 
	* @param P		The point from the grid around which the average is calculated
	* @param radius	The radius, whis is taken into account in average calculation
	* @return the average
	*/
	float getAverage(TCoord2D P, float radius);
	
	/**
	* Returns an average value that surround the point @P 
	* @param P		The point from the grid around which the average is calculated
	* @param radius	The radius, whis is taken into account in average calculation
	* @return the average
	*/
	float getAverage(TPixelCoord P, float radius);
	
	/**
	 * Returns min value that surround the point @P 
	 * @param P		The point from the grid around which the average is calculated
	 * @return the minimum
	 */
	float getMin(TPixelCoord P);
	
	/**
	* Sets line to map. 
	* The line is discreticed and the whole line area is set to @height
	* @param l		The line to be set
	* @param height	The height the pixels are set 
	*/
	void setLine2Map(CLine l, float height);


	void setCircle2Map ( int x, int y, int radius );
	
	/**
	 * Computes a gradient of the height matrix
	 * Handles now all neccessary memory stuff for *out!
	 *** If *out == NULL the memory is reserved, else freed and reserved
	 *** Not efficient but easy ;)
	 */
	int calculateGradient(float *out, int &width, int &height,bool setToGrid=false);
	
	/**
	* saves the map to the text file
	* @param *filename the file name to save to
	*/
	int saveGroundModelMapToTextFile(const char *filename);

	/**
	* saves the map to the text file for matlab, without any information about resolutio, or origins
	* @param *filename the file name to save to
	*/
	int saveGroundModelMapToTextFileForMatlab(const char *filename);
	
	/**
	* Loads pointdata set from textfile and converts the data to the height map
	* The data must be if form "%f %f %f\n" in order x,y,z; The Z is the height
	* @param *filename		file to open
	* @param zero_level		The value from the data which is set to zero level. Note 
	*						that everything below this is set to zero.
	* @param max			The maximum allowed height
	*/
	int load3DTextDataset(const char *filename, float zero_level, float max);
	
	/**
	* Loads the map from file, using the same format as @saveGroundModelMapToTextFile
	* 
	*/
	int loadFromTextFile(const char *file);
	/**
	* Calculates interpolation over the whole map
	*/
	int interpolateGroundModelMap();
	
	/**
	* Calculates interpolation for pixel (x,y)
	*/
	int interpolateGroundModelMap(int x,int y);
	/**
	* Computes interpolation for those pixels that exeeds the
	* maxGrad value
	*/
	void gradientFilter(float maxGrad);

	bool checkConstraints(float *point){
			if(point[0]>xMinConst && point[0]<xMaxConst &&
						point[1]>yMinConst && point[1] < yMaxConst &&
						point[2]>zMinConst && point[2]< zMaxConst) return true;
			return false;		
	}
	
	void setXConstraints(float xMin, float xMax){
		xMaxConst = xMax; xMinConst = xMin;
	}
	void setYConstraints(float yMin, float yMax){
			yMaxConst = yMax; yMinConst = yMin;
	}
	void setZConstraints(float zMin, float zMax){
			fprintf(stderr,"Setting zmin=%.2f zmax=%.2f\n",zMin,zMax);
			zMaxConst = zMax; zMinConst = zMin;
	}
	void setData(float *data, int size){
		for(int i=0;i<size;i++){
				if(checkConstraints(&data[3*i])){
						updateMaximumHeight(TCoord2D(data[3*i],data[3*i+1]), data[3*i+2]);
						//updateHeight(TCoord2D(data[3*i],data[3*i+1]), data[3*i+2]);
				}
		}
	}
	
	float getSize(){
			return this->size_x;
	}
	/**
	* Simple interface to create complicated grid ;)
	* @p dim is the size of the grid in meters (x,y)
	* @p reso is the size of the cell in meters
	* The origin is placed into the center of the grid!
	*/
	void createNewSimple(float dim, float reso){
			TPixelCoord tc;	
			tc.origin(dim,dim,reso,(int)ORIGIN_CENTER);	
			initGroundModelMap(dim,dim,reso,tc);		
	}
	
	
	
};          


#endif


